{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from transformations import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "avg quat [-0.96121841 -0.11072778 -0.02517774  0.25132569] avg euler (0.20268941160030826, 0.10424880585182261, -0.5008701140561233)\n"
     ]
    }
   ],
   "source": [
    "rpys = np.random.randn(1000, 3)*0.1 + np.array([0.2, 0.1, -0.5])\n",
    "quats = []\n",
    "for rpy in rpys:\n",
    "    quat = quaternion_from_euler(rpy[0], rpy[1], rpy[2])\n",
    "    if np.random.randint(0, 2) == 0:\n",
    "        quat = - quat\n",
    "    quats.append(quat)\n",
    "quats = np.array(quats)\n",
    "\n",
    "def average_quats(quats):\n",
    "    Q = quats.T\n",
    "    M = Q@Q.T\n",
    "    w, vr = np.linalg.eig(M)\n",
    "    avg_q = vr[:, np.argmax(w)]\n",
    "    return avg_q/np.linalg.norm(avg_q)\n",
    "\n",
    "quat = average_quats(quats)\n",
    "print(\"avg quat\", quat, \"avg euler\", euler_from_quaternion(quat))"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "916dbcbb3f70747c44a77c7bcd40155683ae19c65e1c03b4aa3499c5328201f1"
  },
  "kernelspec": {
   "display_name": "Python 3.8.10 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
